package nape.dynamics;
import cx.Algorithm;
import cx.Allocator;
import nape.callbacks.Callback;
import nape.dynamics.Arbiter;
import nape.geom.VecMath;
import nape.shape.Shape;
import nape.phys.Particle;
import nape.phys.PhysObj;
import nape.util.FastMath;
import nape.Config;

//'newfile' generated imports
import nape.dynamics.Arbiter_false_false_true_true_ShapeShape;

class Arbiter_false_false_true_true_ShapeShape extends Arbiter {
	
	public var next:Arbiter_false_false_true_true_ShapeShape;
	
	
	public var _s1:Shape;
	public var _s2:Shape;
	
	
	
	public function new () {
		super();
	}

	
	
	public inline function retire() {
		if(p1!=null) {
			p1.p_arbiters(false,false,true,true,Shape,Shape).remove(this);
			p2.p_arbiters(false,false,true,true,Shape,Shape).remove(this);
		}
		live  = false;
		woken = true;
		updated = false;
		ignore = false;
		p1 = p2 = null;
		while(!contacts.empty()) { var c = contacts.front(); contacts.pop(); alloc.CxFree(Contact)(c); }
		alloc.CxFree(Arbiter_false_false_true_true_ShapeShape)(this);
		stamp = 0;
	}
	
	
	
	public inline function injectContact(px:Float,py:Float,nx:Float,ny:Float,dist:Float,hash:Int) {
		var c:Contact = null;
		CxIter(cur,contacts, if(hash==cur.hash) { c = cur; break; });
		if(c==null) {
			c = alloc.CxAlloc(Contact)();
			c.cb_rest = c.cb_dyn = c.cb_stat = -1;
			c.hash   = hash;
			c.jnAcc  = c.jtAcc = 0;
			c.sBias  = -1;
			c.fresh  = true;
			contacts.add(c);
		}else c.fresh = false;
		vec_set(c.p,p);
		vec_set(c.n,n);
		c.dist = dist;
		c.updated = true;
	}
	
	
	
	public inline function assign(s1:Shape,s2:Shape,ID:Int) {
		
		
		 p1 = b1 = s1.body;
		 p2 = b2 = s2.body;
		_s1 = s1;
		_s2 = s2;
		id = ID;
		
		p1.p_arbiters(false,false,true,true,Shape,Shape).add(this);
		p2.p_arbiters(false,false,true,true,Shape,Shape).add(this);
		
		calcProperties();
		
		live  = false;
		woken = true;
	}
	
	public inline function calcProperties() {
		restitution = FastMath.sqrt(_s1.material.eCoef     * _s2.material.eCoef    );
		dyn_fric    = FastMath.sqrt(_s1.material.dyn_fric  * _s2.material.dyn_fric );
		stat_fric   = FastMath.sqrt(_s1.material.stat_fric * _s2.material.stat_fric);
	}
	
	
	
	
	
	
	
	
	
	public inline function preStep() {
		var mass_sum = (if(false) p1.smass else 0)
					 + (if(true) p2.smass else 0);
		var pre = null;
		CxIter(c,contacts,{
			if (!c.updated) {
				cxiterator = contacts.erase(pre,cxiterator);
				alloc.CxFree(Contact)(c);
				continue;
			}
			c.updated = false;
			
			var kn = mass_sum;
			var kt = mass_sum;
			
			if(false)  {
		vec_sub(c.p, p1.p, c.r1)
		var rcn = vec_cross(c.r1, c.n);
		var rct = vec_dot  (c.r1, c.n);
		kn += b1.smoment * rcn*rcn;
		kt += b1.smoment * rct*rct;
	};
			if(true)  {
		vec_sub(c.p, p2.p, c.r2)
		var rcn = vec_cross(c.r2, c.n);
		var rct = vec_dot  (c.r2, c.n);
		kn += b2.smoment * rcn*rcn;
		kt += b2.smoment * rct*rct;
	};
			c.nMass = 1.0/kn;
			c.tMass = 1.0/kt;
			
			vec_new(vr);  {
		vrx = (if(false) c.r1y * b1.w else 0)
		     - (if(true) c.r2y * b2.w else 0)
		     - (if(false) p1.vx        else 0)
		     + (if(true) p2.vx        else 0);
		
		vry = (if(true) c.r2x * b2.w else 0)
		     - (if(false) c.r1x * b1.w else 0)
		     - (if(false) p1.vy        else 0)
		     + (if(true) p2.vy        else 0);
	}
			var vdot = vec_dot(c.n, vr);
			c.bias = c.dist+Config.OVERLAP;
			var nb = Config.MAX_BIAS_COEF - Config.DEL_BIAS_COEF / (1 + Config.VEL_BIAS_COEF * vdot*vdot - c.bias*Config.BIAS_PEN_COEF);
			if(c.sBias==-1) c.sBias = nb;
			else            c.sBias = Config.INT_BIAS_COEF * c.sBias + Config.SUB_BIAS_COEF * nb;
			
			if(c.bias>0) c.bias = 0;
			else         c.bias = -c.sBias*c.bias;
			c.jBias = 0;
			
			if(c.cb_rest!=-1) c.bounce = vdot*c.cb_rest;
			else              c.bounce = vdot*restitution;
			
			vdot = vec_cross(c.n,vr);
			if(vdot*vdot>Config.STATIC_VELSQ) {
				if(c.cb_dyn!=-1) c.friction = c.cb_dyn;
				else             c.friction = dyn_fric;
			}else {
				if(c.cb_stat!=-1) c.friction = c.cb_stat;
				else              c.friction = stat_fric;
			}
			
			c.cb_rest = c.cb_dyn = c.cb_stat = -1;
				
			pre = cxiterator;
		});
	}
	
	
	
	
	
	
	
	public inline function applyImpulseCache() {
		CxIter(c,contacts,{
			c.pjnAcc = c.jnAcc;
			c.pjtAcc = c.jtAcc;
			c.pjBias = c.jBias;
			 {
		var jx = c.nx*c.jnAcc - c.ny*c.jtAcc;
		var jy = c.ny*c.jnAcc + c.nx*c.jtAcc;
		
		if(false) vec_subeq(p1.v, j, p1.imass);
		if(true) vec_addeq(p2.v, j, p2.imass);
		if(false) b1.w -= b1.imoment * vec_cross(c.r1,j);
		if(true) b2.w += b2.imoment * vec_cross(c.r2,j);
	};
		});
	}
	
	
	
	
	
	public inline function applyImpulsePos()  {
		CxIter(c,contacts,{
			vec_new(vr);  {
		vrx = (if(false) c.r1y * b1.bw else 0)
		     - (if(true) c.r2y * b2.bw else 0)
		     - (if(false) p1.bvx        else 0)
		     + (if(true) p2.bvx        else 0);
		
		vry = (if(true) c.r2x * b2.bw else 0)
		     - (if(false) c.r1x * b1.bw else 0)
		     - (if(false) p1.bvy        else 0)
		     + (if(true) p2.bvy        else 0);
	}
			
			
			var jbn = (c.bias - vec_dot(vr,c.n)) * c.nMass;
			var jbnOld = c.jBias;
			var cjBias = jbnOld + jbn; if (cjBias < 0) cjBias = 0;
			var jb = cjBias - jbnOld;
			c.jBias = cjBias;
			
			 {
		var jx = c.nx*jb - c.ny*0;
		var jy = c.ny*jb + c.nx*0;
		
		if(false) vec_subeq(p1.bv, j, p1.imass);
		if(true) vec_addeq(p2.bv, j, p2.imass);
		if(false) b1.bw -= b1.imoment * vec_cross(c.r1,j);
		if(true) b2.bw += b2.imoment * vec_cross(c.r2,j);
	};
			
			 {
		vrx = (if(false) c.r1y * b1.w else 0)
		     - (if(true) c.r2y * b2.w else 0)
		     - (if(false) p1.vx        else 0)
		     + (if(true) p2.vx        else 0);
		
		vry = (if(true) c.r2x * b2.w else 0)
		     - (if(false) c.r1x * b1.w else 0)
		     - (if(false) p1.vy        else 0)
		     + (if(true) p2.vy        else 0);
	}
			
			var jn = ((if(false) c.bounce else 0 ) + vec_dot(c.n,vr))*c.nMass;
			var jnOld = c.jnAcc;
			var cjnAcc = jnOld - jn; if (cjnAcc<0) cjnAcc = 0;
			jn = cjnAcc - jnOld;
			c.jnAcc = cjnAcc;
			
			
			var jt = (vec_cross(c.n,vr)) * c.tMass;
			var jtMax = c.friction * c.jnAcc;
			var jtOld = c.jtAcc;
			var cjtAcc = jtOld - jt; if (cjtAcc>jtMax) cjtAcc = jtMax else if(cjtAcc<-jtMax) cjtAcc = -jtMax;
			jt = cjtAcc - jtOld;
			c.jtAcc = cjtAcc;

			 {
		var jx = c.nx*jn - c.ny*jt;
		var jy = c.ny*jn + c.nx*jt;
		
		if(false) vec_subeq(p1.v, j, p1.imass);
		if(true) vec_addeq(p2.v, j, p2.imass);
		if(false) b1.w -= b1.imoment * vec_cross(c.r1,j);
		if(true) b2.w += b2.imoment * vec_cross(c.r2,j);
	};
		});
	}
	public inline function applyImpulseVel()  {
		CxIter(c,contacts,{
			vec_new(vr);  {
		vrx = (if(false) c.r1y * b1.bw else 0)
		     - (if(true) c.r2y * b2.bw else 0)
		     - (if(false) p1.bvx        else 0)
		     + (if(true) p2.bvx        else 0);
		
		vry = (if(true) c.r2x * b2.bw else 0)
		     - (if(false) c.r1x * b1.bw else 0)
		     - (if(false) p1.bvy        else 0)
		     + (if(true) p2.bvy        else 0);
	}
			
			
			var jbn = (c.bias - vec_dot(vr,c.n)) * c.nMass;
			var jbnOld = c.jBias;
			var cjBias = jbnOld + jbn; if (cjBias < 0) cjBias = 0;
			var jb = cjBias - jbnOld;
			c.jBias = cjBias;
			
			 {
		var jx = c.nx*jb - c.ny*0;
		var jy = c.ny*jb + c.nx*0;
		
		if(false) vec_subeq(p1.bv, j, p1.imass);
		if(true) vec_addeq(p2.bv, j, p2.imass);
		if(false) b1.bw -= b1.imoment * vec_cross(c.r1,j);
		if(true) b2.bw += b2.imoment * vec_cross(c.r2,j);
	};
			
			 {
		vrx = (if(false) c.r1y * b1.w else 0)
		     - (if(true) c.r2y * b2.w else 0)
		     - (if(false) p1.vx        else 0)
		     + (if(true) p2.vx        else 0);
		
		vry = (if(true) c.r2x * b2.w else 0)
		     - (if(false) c.r1x * b1.w else 0)
		     - (if(false) p1.vy        else 0)
		     + (if(true) p2.vy        else 0);
	}
			
			var jn = ((if(true) c.bounce else 0 ) + vec_dot(c.n,vr))*c.nMass;
			var jnOld = c.jnAcc;
			var cjnAcc = jnOld - jn; if (cjnAcc<0) cjnAcc = 0;
			jn = cjnAcc - jnOld;
			c.jnAcc = cjnAcc;
			
			
			var jt = (vec_cross(c.n,vr)) * c.tMass;
			var jtMax = c.friction * c.jnAcc;
			var jtOld = c.jtAcc;
			var cjtAcc = jtOld - jt; if (cjtAcc>jtMax) cjtAcc = jtMax else if(cjtAcc<-jtMax) cjtAcc = -jtMax;
			jt = cjtAcc - jtOld;
			c.jtAcc = cjtAcc;

			 {
		var jx = c.nx*jn - c.ny*jt;
		var jy = c.ny*jn + c.nx*jt;
		
		if(false) vec_subeq(p1.v, j, p1.imass);
		if(true) vec_addeq(p2.v, j, p2.imass);
		if(false) b1.w -= b1.imoment * vec_cross(c.r1,j);
		if(true) b2.w += b2.imoment * vec_cross(c.r2,j);
	};
		});
	}
	
}